#pragma once
#include "gtest/gtest.h"
#include "DataStructure/Common/Real.h"
#include "DBManager/DBServiceFactory.h"
// #include "SGFem/Pre/Assembly/Assembly.h"
// #include "Utility/Algebra/MathTools.h"
// #include "SGFem/Pre/Assembly/CoordinateProcessor.h"
// #include "../../Factory/ElementCalculatorFactory.h"
#include "../CfastCalculator.h"
#include "Utility/Algebra/MathTools.h"
#include "Utility/Algebra/Matrix.h"
#include "Utility/Algebra/Tools.h"
// #include "DataStructure/Input/Property/PBEAMData.h"
#include "DataStructure/Input/Property/PFASTData.h"
#include "DataStructure/Input/Element/CFASTData.h"
#include "DBManager/IService/Input/Property/IPFASTService.h"
#include "SGFem/Constraint/RElement/RBE3Calculator.h"

using SG::Algebra::Matrix;
using SG::Algebra::Vector3D;
using SG::Algebra::Point;
using SG::DataStructure::Common::Real;
using SG::DataStructure::Common::TOLERANCE;
using namespace SG::DataStructure::FEM;
using namespace SG::DBManager;
using SG::Algebra::Matrixd;
// using SG::FEMSolver::AssemblyEigen;
// using SG::FEMSolver::CoordinateProcessor;
// using SG::FEMSolver::Element::ElementCalculatorFactory;
using SG::FEMSolver::Element::CfastCalculator;

TEST (CfastCalculatorTest, ComputeStiffness)
{
    PFASTData         pFast;
    CFASTData         eleData;
    // DBServiceFactorySP dbServiceFactory;
    // dbServiceFactory->get<IPFASTService> () ->FindById (1, pFast);
    pFast.m_KT[0]=1.0e5;
    pFast.m_KT[1]=4.6e4;
    pFast.m_KT[2]=1.23e4;
    pFast.m_KR[0]=1100;
    pFast.m_KR[1]=300;
    pFast.m_KR[2]=1300;
    // pFast.m_KT[0]=20000;
    // pFast.m_KT[1]=10000;
    // pFast.m_KT[2]=0;
    // pFast.m_KR[0]=0;
    // pFast.m_KR[1]=0;
    // pFast.m_KR[2]=0;
    
    // 粱单元刚度矩阵计算
    Matrix  T (3, 3);
    Matrix  klTest (12, 12);
    SG::Algebra::Point GA{0.0, 0.0, 0.0};
    SG::Algebra::Point GB{4.0, 0.0, 0.0};
    Vector3D n1{1.0, 0.0, 0.0};
    Vector3D n2{0.0, 1.0, 0.0};
    Real L = length (GA, GB);
    if (L > 1.0e-6)
    {

        SG::Algebra::computeLocalCoordTrans (n1, n2, _OUT T);
        CfastCalculator calculator;
        calculator.ComputeStiffness (L, pFast, _OUT klTest);
    }
 
    Matrixd TBeam (12, 12);
    // ToDo GA 和 GB 非投影点时，包含位移输出坐标系

    TBeam.BlockFill (0, 0, T);
    TBeam.BlockFill (3, 3, T);
    TBeam.BlockFill (6, 6, T); 
    TBeam.BlockFill (9, 9, T);

    klTest = TBeam.Transpose () * klTest * TBeam;
    

    // Target data
    const Matrixd klTarget (12, 12 ,{
        100000,  	0,	    0,	    0,	    0,	      0,	   -100000,	  0,	    0,	    0,	    0,	    0,
        0,	        46000,	0,	    0,	    0,	      92000,	0,	     -46000,	0,	    0,	    0,	    92000,
        0,	        0,	    12300,	0,	   -24600,	  0,	    0,	      0,	   -12300,	0,	   -24600,	0,
        0,	        0,	    0,	    1100,	0,	      0,	    0,	      0,	    0,	   -1100,	0,	    0,
        0,	        0,	   -24600,	0,	    49500,	  0,	    0,	      0,	    24600,	0,	    48900,	0,
        0,	        92000,	0,	    0,	    0,	      185300,	0,	     -92000,	0,	    0,	    0,	    182700,
        -100000,	    0,	    0,	    0,	    0,	      0,	    100000,	  0,	    0,	    0,	    0,	    0,
        0,	       -46000,	0,	    0,	    0,	     -92000,	0,	      46000,	0,	    0,	    0,	   -92000,
        0,	        0,	   -12300,	0,	    24600,	  0,	    0,	      0,	    12300,	0,	    24600,	0,
        0,	        0,	    0,	   -1100,	0,	      0,	    0,	      0,	    0,	    1100,	0,	    0,
        0,	        0,	   -24600,	0,	    48900,	  0,	    0,	      0,	    24600,	0,	    49500,	0,
        0,	        92000,	0,	    0,	    0,	      182700,	0,	     -92000,	0,	    0,	    0,	    185300
                
    });
    
    // Check
    ASSERT_TRUE (klTarget == klTest);
}

TEST (CfastCalculatorTest, ComputeAuxNode)
{
    // Input data
    Point GALocal {11.520442189225816,5.4570515633174921,0.0};
    Matrix Tshell(3,3,{0.24253562503633297,-0.97014250014533188, 0,
                       0.97014250014533188, 0.24253562503633297,-0,
                       0,                   0,                   1});
    Matrix T(3, 3, {-0.044873396889511287, -0.044873396889511287,  0.99798434682273096,
                     0.99798637825159653,  -0.0020136217484036014, 0.044782947684496088,
                    -0,                     0.99798434682273096,   0.044873396889511287});
    Real   a = 0.0886226925452758;
    SG::Algebra::PointArray pGAvirual3DTest;
    
    // Test data
    Matrix auxPointLocalTLarge(4,2,{11.413101712383298,5.521750754187007,
        11.455742998356301,5.349711086474974,
        11.627782666068335,5.3923523724479772,
        11.585141380095331,5.5643920401600102});
        
    SG::Algebra::PointArray pGAvirual3DLarge(4);
    pGAvirual3DLarge[0] = {1.2575492710054219,-0.22155673136318949,-1.5708778053342869};
    pGAvirual3DLarge[1] = {1.2575492710054219,0.22155673136318949,-1.5708778053342869};
    pGAvirual3DLarge[2] = {1.570877805334298,0.22155673136318949,-1.257549271005413};
    pGAvirual3DLarge[3] = {1.570877805334298,-0.22155673136318949,-1.257549271005413};
    
    Matrix AuxGlobal3DLarge (4, 3, {-2.5888013244302295,12.41155630012609,0,
        -2.4115563001260902,12.41119867556977,0,
        -2.4111986755697705,12.58844369987391,0,
        -2.5884436998739098,12.588801324430229,0});
            
    Matrix auxPointLocalTest;
    CfastCalculator::ComputeAuxNode (GALocal, Tshell, T, a, auxPointLocalTest);
    
    Matrix T32(3, 2, {0.24253562503633297,-0.97014250014533188,
                    0.97014250014533188, 0.24253562503633297,
                    0,                   0});

    Matrix AuxGlobal3DTest = auxPointLocalTest * T32.Transpose();
    // Check
    ASSERT_TRUE (auxPointLocalTLarge == auxPointLocalTest);
    ASSERT_TRUE (AuxGlobal3DLarge    == AuxGlobal3DTest);
}

TEST (CfastCalculatorTest, ComputeQuadWeights)
{
        // Input data
    SG::Algebra::Matrix     mAux2D(4,2,{-0.22155673136318949,-0.22155673136318949,
                                              0.22155673136318949,-0.22155673136318949,
                                              0.22155673136318949,0.22155673136318949,
                                             -0.22155673136318949,0.22155673136318949});

    SG::Algebra::Matrix     shellLocal(4,2,{-1,-0.99999998288572844,
                                              1,-0.99999998288572844,
                                              1,0.99999998288572844,
                                             -1,0.99999998288572844});

    std::vector<Real> wright(4);
    std::vector<std::vector<Real>> WrightTest(4,(std::vector<Real>(4)));
    std::vector<std::vector<Real>> WrightTarget(4,(std::vector<Real>(4)));

    // Test data
    WrightTarget[0] = {0.37305021314264913,0.23772815443483672,0.15149347988356857,0.23772815253894561};
    WrightTarget[1] = {0.23772815443483672,0.37305021314264913,0.23772815253894561,0.15149347988356857};
    WrightTarget[2] = {0.15149347988356857,0.23772815253894561,0.37305021314264913,0.23772815443483672};
    WrightTarget[3] = {0.23772815253894561,0.15149347988356857,0.23772815443483672,0.37305021314264913};
    for (std::size_t size = 0; size < 4; size++)
    {
        CfastCalculator::ComputeQuadWeights (mAux2D(size, 0), mAux2D(size, 1), shellLocal, wright);
        WrightTest[size] = wright;
    }
    // Check
    ASSERT_TRUE (WrightTarget == WrightTest);
}

//三角形模型权系数测试
TEST (CfastCalculatorTest, ComputeTriWeights)
{
        // Input data
    SG::Algebra::Matrix     mAux2D(4,2,{11.413101712383298,5.521750754187007,
                                            11.455742998356301,5.349711086474974,
                                            11.627782666068335,5.3923523724479772,
                                            11.585141380095331,5.5643920401600102});

    SG::Algebra::Matrix     shellLocal(3,2,{0,                  0,
                                            20.615528128088304, 0,
                                            12.126781251816649, 13.339459376998313});

    std::vector<Real> wright(3);
    std::vector<std::vector<Real>> WrightTest(4,(std::vector<Real>(3)));
    std::vector<std::vector<Real>> WrightTarget(4,(std::vector<Real>(3)));

    // Test data
    WrightTarget[0] = {0.27593690500348178,0.31012197503566347,0.41394111996085475};
    WrightTarget[1] = {0.2791790486029927 ,0.31977688092293216,0.40104407047407514};
    WrightTarget[2] = {0.26951764045106363,0.32624166132797283,0.40424069822096353};
    WrightTarget[3] = {0.26627549685155277,0.31658675544070414,0.41713774770774315};
    for (std::size_t size = 0; size < 4; size++)
    {
        CfastCalculator::ComputeTriWeights (mAux2D(size, 0), mAux2D(size, 1), shellLocal, wright);
        WrightTest[size] = wright;
    }
    // Check
    ASSERT_TRUE (WrightTarget == WrightTest);
}